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Abstract 

Probabilistic sampling methods have become very 
popular to solve single-shot path planning problems. 
Rapidly-exploring Random Trees (RRTs) in particu- 
lar have been shown to be very efficient in solving 
high dimensional problems. Even though several RRT 
variants have been proposed to tackle the dynamic 
replanning problem, these methods only perform well 
in environments with infrequent changes. This pa- 
per addresses the dynamic path planning problem by 
combining simple techniques in a multi-stage prob- 
abilistic algorithm. This algorithm uses RRTs as an 
initial solution, informed local search to fix unfeasible 
paths and a simple greedy optimizer. The algorithm 
is capable of recognizing when the local search is 
stuck, and subsequently restart the RRT. We show that 
this combination of simple techniques provides better 
responses to a highly dynamic environment than the 
dynamic RRT variants. 

Index Terms 

artificial intelligence; motion planning; RRT; Multi- 
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1. Introduction 

The dynamic path-planning problem consists in 
finding a suitable plan for each new configuration 
of the environment by recomputing a collision free 
path using the new information available at each time 
step [5]. This kind of problem can be found for 
example by a robot trying to navigate through an area 



crowded with people, such as a shopping mall or super- 
market. The problem has been addressed widely in its 
several flavors, such as cellular decomposition of the 
configuration space [12], partial environmental knowl- 
edge ifTTI . high-dimensional configuration spaces [6] or 
planning with non-holonomic constraints [ 8 1 . However, 
simpler variations of this problem are complex enough 
that cannot be solved with deterministic techniques, 
and therefore they are worthy to study. 

This paper is focused on finding and traversing a 
collision-free path in two dimensional space, for a 
holonomic robot Q without kinodynamic restrictions ^ 
in two different scenarios: 

• Dynamic environment: several unpredictably 
moving obstacles or adversaries. 

• Partially known environment: some obstacles only 
become visible when approached by the robot. 

Besides from one (or few) new obstacle(s) in the 
second scenario we assume that we have perfect in- 
formation of the environment at all times. 

We will focus on continuous space algorithms and 
won't consider algorithms that use discretized repre- 
sentations of the configuration space, such as D* fl2l . 
because for high dimensional problems, the config- 
uration space becomes intractable in terms of both 
memory and computation time, and there is the extra 
difficulty of calculating the discretization size, trading 
off accuracy versus computational cost. 

The offline RRT is efficient at finding solutions but 
they are far from being optimal, and must be post- 
processed for shortening, smoothing or other qualities 

1 . A holonomic robot is a robot in which the controllable degrees 
of freedom is equal to the total degrees of freedom. 

2. Kinodynamic planning is a problem in which velocity and 
acceleration bounds must be satisfied 



that might be desirable in each particular problem. 
Furthermore, replanning RRTs are costly in terms of 
computation time, as well as evolutionary and cell- 
decomposition approaches. Therefore, the novelty of 
this work is the mixture of the feasibility benefits of the 
RRTs, the repairing capabilities of local search, and the 
computational inexpensiveness of greedy algorithms, 
into our lightweight multi-stage algorithm. 

In the following sections, we present several path 
planning methods that can be applied to the problem 
described above. In section 12.11 we review the basic 
offline, single-query RRT, a probabilistic method that 
builds a tree along the free configuration space until 
it reaches the goal state. Afterward, we introduce the 
most popular replanning variants of the RRT: ERRT in 
section |Z2] DRRT in section [23] and MP-RRT in sec- 
tion 2.4 Then, in section [3] we present our new hybrid 
multi-stage algorithm with the experimental results and 
comparisons in section |4] Finally, the conclusions and 
further work are discussed in section |5] 

2. Previous and Related Work 

2.1. Rapidly-Exploring Random Tree 

One of the most successful probabilistic sampling 
methods for offline path planning currently in use, is 
the Rapidly-exploring Random Tree (RRT), a single- 
query planner for static environments, first introduced 
in [9|. RRTs work towards finding a continuous path 
from a state q^it to a state q goa i in the free configu- 
ration space Cf ree , by building a tree rooted at qinit- 
A new state q ran d is uniformly sampled at random 
from the configuration space C. Then the nearest 
node, q near , in the tree is located, and if q ran d and 
the shortest path from q ran d to q near are in Cf ree , 
then q ran d is added to the tree. The tree growth is 
stopped when a node is found near q goa i- To speed up 
convergence, the search is usually biased to q goa i with 
a small probability. 

In 0, two new features are added to RRTs. First, 
the EXTEND function is introduced, which, instead 
of trying to add directly q ran d to the tree, makes a 
motion towards q ran d and tests for collisions. 

Then a greedier approach is introduced, which re- 
peats EXTEND until an obstacle is reached. This 
ensures that most of the time, we will be adding states 
to the tree, instead of just rejecting new random states. 
The second extension is the use of two trees, rooted at 
qinit and q goa i, which are grown towards each other. 
This significantly decreases the time needed to find a 
path. 



2.2. ERRT 

The execution extended RRT presented in [3 1 intro- 
duces two RRTs extensions to build an on-line planner: 
the waypoint cache and the adaptive cost penalty 
search, which improves re-planning efficiency and the 
quality of generated paths. The waypoint cache is 
implemented by keeping a constant size array of states, 
and whenever a plan is found, all the states in the 
plan are placed in the cache with random replacement. 
Then, when the tree is no longer valid, a new tree must 
be grown, and there are three possibilities for choosing 
a new target state. With probability P[goal], the goal 
is chosen as the target; With probability P[waypoint], 
a random waypoint is chosen, and with remaining 
probability a uniform state is chosen as before. Values 
used in |3| are P[goal]= 0.1 and P[waypoint]= 0.6. 
In the other extension — the adaptive cost penalty 
search — the planner dynamically modifies a parame- 
ter (3 to help it finding shorter paths. A value of 1 for (3 
will always extend from the root node, while a value of 
is equivalent to the original algorithm. Unfortunately, 
the solution presented in [3| lacks of implementation 
details and experimental results on this extension. 

2.3. Dynamic RRT 

The Dynamic Rapidly-exploring Random Tree 
(DRRT) described in (4) is a probabilistic analog to 
the widely used D* family of algorithms. It works 
by growing a tree from q goa i to q mlt . The principal 
advantage is that the root of the tree does not have 
to be changed during the lifetime of the planning 
and execution. Also, in some problem classes the 
robot has limited range sensors, thus moving obstacles 
(or new ones) are typically near the robot and not 
near the goal. In general, this strategy attempts to 
trim smaller branches and farther away from the root. 
When new information concerning the configuration 
space is received, the algorithm removes the newly- 
invalid branches of the tree, and grows the remaining 
tree, focusing, with a certain probability(empirically 
tuned to 0.4 in J4)) to a vicinity of the recently 
trimmed branches, by using the a similar structure 
to the waypoint cache of the ERRT. In experimental 
results DRRT vastly outperforms ERRT. 

2.4. MP-RRT 

The Multipartite RRT presented in lfT4l is another 
RRT variant which supports planning in unknown 
or dynamic environments. The MP-RRT maintains a 
forest F of disconnected sub-trees which lie in Cf ree , 



but which are not connected to the root node q roo t 
of T, the main tree. At the start of a given planning 
iteration, any nodes of T and F which are no longer 
valid are deleted, and any disconnected sub-trees which 
are created as a result are placed into F. With given 
probabilities, the algorithm tries to connect T to a 
new random state, to the goal state, or to the root 
of a tree in F. In [14|, a simple greedy smoothing 
heuristic is used, that tries to shorten paths by skipping 
intermediate nodes. The MP-RRT is compared to an 
iterated RRT, ERRT and DRRT, in 2D, 3D and 4D 
problems, with and without smoothing. For most of the 
experiments, MP-RRT modestly outperforms the other 
algorithms, but in the 4D case with smoothing, the 
performance gap in favor of MP-RRT is much larger. 
The authors explained this fact due to MP-RRT being 
able to construct much more robust plans in the face 
of dynamic obstacle motion. Another algorithm that 
utilizes the concept of forests is the Reconfigurable 
Random Forests (RRF) presented in ifTUl . but without 
the success of MP-RRT. 

3. A Multi-stage Probabilistic Algorithm 

In highly dynamic environments, with many (or a 
few but fast) relatively small moving obstacles, regrow- 
ing trees are pruned too fast, cutting away important 
parts of the trees before they can be replaced. This 
reduce dramatically the performance of the algorithms, 
making them unsuitable for these class of problems. 
We believe that a better performance could be obtained 
by slightly modifying a RRT solution using simple 
obstacle-avoidance operations on the new colliding 
points of the path by informed local search. Then, 
the path could be greedily optimized if the path has 
reached the feasibility condition. 

3.1. Problem Formulation 

At each time-step, the proposed problem could be 
defined as an optimization problem with satisfiability 
constraints. Therefore, given a path our objective is 
to minimize an evaluation function (i.e. distance, time, 
or path-points), with the C/ ree constraint. Formally, let 
the path p = p\pi ■ ■ -p n a sequence of points, where 
Pi G K™ a n-dimensional point (pi = qi n it,Pn — 
Qgoai), Ot € O the set of obstacles positions at time 
t, and eval : R" x O i— ► K an evaluation function 
of the path depending on the object positions. Then, 
our ideal objective is to obtain the optimum p* path 
that minimize our eval function within a feasibility 
restriction in the form 



p* = argmin[eval{p,Ot)\ with feas(p, Ot) — Ct ree 
p 

(1) 

where feas(-, ■) is a feasibility function that equals 
to C f ree iff the path p is collision free for the obstacles 
O t . For simplicity, we use very naive euai(v) an d 
feas(-, ■) functions, but this could be extended easily 
to more complex evaluation and feasibility functions. 
The used feas(p, Ot) function assumes that the robot 
is a punctual object (dimensionless) in the space, and 
therefore, if all segments PiPi+i of the path do not 
collide with any object o,j E O t , we say that the 
path is in C/ ree . The eval(p, Ot) function will be the 
length of the path, i.e. the sum of the distances between 
consecutive points. This could be easily changed to any 
metric such as the time it would take to traverse this 
path, accounting for smoothness, clearness or several 
other optimization criteria. 

3.2. A Multi-stage Probabilistic Strategy 

If solving equation [T] is not a simple task in static 
environments, solving dynamic versions turns out to 
be even more difficult. In dynamic path planning we 
cannot wait until reaching the optimal solution because 
we must deliver a "good enough" plan within some 
time quantum. Then, a heuristic approach must be 
developed to tackle the on-line nature of the problem. 



The heuristic algorithms presented in sections 2.2 



2.3 and 2.4 extend a method developed for static 
environments, which produce a poor response to highly 
dynamic environments and an unwanted complexity of 
the algorithms. 

We propose a multi-stage combination of three 
simple heuristic probabilistic techniques to solve each 
part of the problem: feasibility, initial solution and 
optimization. 

3.2.1. Feasibility. The key point in this problem is 
the hard constraint in equation [T] which must be met 
before even thinking about optimizing. The problem 
is that in highly dynamic environments a path turns 
rapidly from feasible to unfeasible — and the other 
way around — even if our path does not change. We 
propose a simple informed local search to obtain paths 
in Cf r ee- The idea is to randomly search for a Cf ree 
path by modifying the nearest colliding segment of the 
path. As we include in the search some knowledge of 
the problem, the informed term is coined to distinguish 
it from blind local search. The details of the operators 
used for the modification of the path are described in 
section 13.31 



3.3. Algorithm Implementation 
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Algorithm 1 . Main() 

Require: q ro bot is the current robot position 
Require: q goa i *— is the goal position 

l: while q robot ^ q g0 ai do 

2: updateWorld(time) 

3: process(iime) 

The multi-stage algorithm proposed in this paper 
works by alternating environment updates and path 
planning, as can be seen in Algorithm [Tl The first stage 
of the path planning (see Algorithm |2]i is to find an 
initial path using a RRT technique, ignoring any cuts 
that might happen during environment updates. Thus, 
the RRT ensures that the path found does not collide 
with static obstacles, but might collide with dynamic 
obstacles in the future. When a first path is found, the 
navigation is done by alternating a simple informed 
local search and a simple greedy heuristic as is shown 
in Figure [T] 



Algorithm 2. process(iime) 



Figure 1 . A Multi-stage Strategy for Dynamic 
Path Planning. This figure describes the life-cycle 
of the multi-stage algorithm presented here. The 
RRT, informed local search, and greedy heuristic 
are combined to produce an expensiveness solu- 
tion to the dynamic path planning problem. 



3.2.2. Initial Solution. The problem with local search 
algorithms is that they repair a solution that it is 
assumed to be near the feasibility condition. Trying to 
produce feasible paths from scratch with local search 
(or even with evolutionary algorithms lfl3l ) is not a 
good idea due the randomness of the initial solution. 
Therefore, we propose feeding the informed local 
search with a standard RRT solution at the start of 
the planning, as can be seen in figure [T] 

3.2.3. Optimization. Without an optimization criteria, 
the path could grow infinitely large in time or size. 
Therefore, the eval(-, •) function must be minimized 
when a (temporary) feasible path is obtained. A simple 
greedy technique is used here: we test each point in 
the solution to check if it can be removed maintaining 
feasibility, if so, we remove it and check the following 
point, continuing until reaching the last one. 



Require: q ro bot 
Require: q sta rt 
Require: q goa i 
Require: T init 

position 
Require: T goa i 

position 
Require: path 

merged RRTs 

qrobot * qstart 

T mit .init(q r obot) 
T g0 ai-init{q goa i) 



is the current robot position 
is the starting position 
is the goal position 
is the tree rooted at the robot 

- is the tree rooted at the goal 

is the path extracted from the 



while time elapsed < time do 
if first path not found then 

RRT(Tj n j t , Tgoai) 
else 

if path is not collision free then 

firstCol *— collision point closest to robot 

aic(path, firstCol) 
mut(path, firstCol) 
postProcess(paift) 



The second stage is the informed local search, which 
is a two step function composed by the arc and mutate 
operators (Algorithms [3] and 0J. The first one tries to 
build a square arc around an obstacle, by inserting 
two new points between two points in the path that 
form a segment colliding with an obstacle, as is shown 
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Figure 2. The arc operator. This operator draws 
an offset value A over a fixed interval called vicin- 
ity. Then, one of the two axises is selected to 
perform the arc and two new consecutive points 
are added to the path, m is placed at a ±A of 
the point b and n 2 at ±A of point c, both of them 
over the same selected axis. The axis, sign and 
value of A are chosen randomly from an uniform 
distribution. 



continuing with the navigation phase. 

Algorithm3. arc(path, firstCol) 
Require: vicinity «— some vicinity size 

randDev «— random(— vicinity, vicinity) 
point 1 «— path[firstCol] 
point2 «— path[firstCol+l] 
if random()%2 then 



new Point 1 
newPoint2 
else 

newPointl 
newPoint2 



(pointl [X]+randDev,pointl [Y]) 
(point2 [X]+randDev,point2 [ Y] ) 



(pointl [X],pointl [Y]+randDev) 
(point2 [X] ,point2 [ Y] +randDev) 
if path segments pointl-newPointl-newPoint2- 
point2 are collision free then 

add new points between pointl and point2 
else 

drop new point2 
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Object \ 





■ Vecinity 



Figure 3. The mutation operator. This operator 
draws two offset values A x and A y over a vicinity 
region. Then the same point b is moved in both 
axises from b = [b x ,b y ] to b' = [b x ± A x ,b y ± A y ], 
where the sign and offset values are chosen ran- 
domly from an uniform distribution. 



Algorithm 4. mut(path, firstCol) 
Require: vicinity <— some vicinity size 
1: path[firstCol][X] + 

random(— vicinity, vicinity) 
2: path[firstCol][Y] + 

random(— vicinity, vicinity) 
3: if path segments before and after path [firstCol] are 

collision free then 
4: accept new point 
5: else 

6: reject new point 

The third and last stage is the greedy optimization 
heuristic, which can be seen as a post-processing for 
path shortening, that eliminates intermediate nodes if 
doing so does not create collisions, as is described in 
the Algorithm [3] 



in Figure [2] The second step in the function is a 
mutation operator that moves a point close to an 
obstacle to a random point in the vicinity, as is graph- 
ically explained in Figure [3] The mutation operator is 
inspired by the ones used in the Adaptive Evolutionary 
Planner/Navigator(EP/N) presented in fl3l . while the 
arc operator is derived from the arc operator in the 
Evolutionary Algorithm presented in iffl . 
Even though the local search usually produce good 
results for minor changes in the environment, it does 
not when is faced to significant changes and is quite 
prone to getting stuck in an obstacle. To overcome 
this limitation, our algorithm recognizes this situation, 
and restarts an RRT from the current location, before 



Algorithm 5. postProcess(pai/i) 

l: i <- 

2: while i < path.size()-2 do 

3: if segment path[i] to path[i+2] is collision free 
then 

4: delete path[i+l] 
5: else 

6: i <- i+1 



4. Experiments and Results 

The multi-stage strategy proposed here has been de- 
veloped to navigate highly-dynamic environments, and 



therefore, our experiments should be aimed towards 
that purpose. Therefore, we have tested our algorithm 
in a highly-dynamic situation on two maps, shown in 
figures |4] and [5] For completeness sake, we have tested 
on the same two maps, but modified to be a partially 
known environment. Also, we have ran the DRRT and 
MP-RRT algorithms over the same situations in order 
to compare the performance of our proposal. 

4.1. Experimental Setup 

The first environment for our experiments consists 
on two maps with 30 moving obstacles the same 
size of the robot, with a random speed between 
10% and 55% the speed of the robot. This dynamic 
environments are shown in figures [4] and [5] 



Figure 4. The dynamic environment, Map 1. The 
green square is our robot, currently at the start po- 
sition. The blue squares are the moving obstacles. 
The blue cross is the goal. 

The second environment uses the same maps, but 
with a few obstacles, three to four times the size 
of the robot, that become visible when the robot 
approaches each one of them. This partially known 
environments are shown in figure [6] and [7] 

The three algorithms were ran a hundred times in 
each environment. The cutoff time was five minutes 
for all tests, after which, the robot was considered 
not to have reached the goal. Results are presented 
concerning: 

• success rate: the percentage of times the robot 
arrived to the goal 

• number of nearest neighbor lookups performed 
by each algorithm(N.N.): one of the possible 
bottlenecks for tree-based algorithms 




Figure 5. The dynamic environment, Map 2. The 
green square is our robot, currently at the start po- 
sition. The blue squares are the moving obstacles. 
The blue cross is the goal. 



Figure 6. The partially know environment, Map 1 . 
The green square is our robot, currently at the start 
position. The yellow squares are the suddenly 
appearing obstacles. The blue cross is the goal. 



* number of collision checks performed(C.C), 
which in our specific implementation takes a 
significant percentage of the running time 

• time it took the robot to reach the goal 

4.2. Implementation Details 

The algorithms where implemented in C++ using a 
framework [^] developed by the same authors. 

There are several variations that can be found in the 
literature when implementing RRTs. For all our RRT 

3. MoPa homepage: https://csrg.inf.utfsm.cl/twiki4/bin/view/CSRG/MoPa 




Figure 7. The partially know environment, Map 2. 
The green square is our robot, currently at the start 
position. The yellow squares are the suddenly 
appearing obstacles. The blue cross is the goal. 



variants, the following are the details on where we 
departed from the basics: 

• We always use two trees rooted at qi n u and q goa i- 

• Our EXTEND function, if the point cannot be 
added without collisions to a tree, adds the mid 
point between the nearest tree node and the near- 
est collision point to it. 

• In each iteration, we try to add the new randomly 
generated point to both trees, and if successful in 
both, the trees are merged, as proposed in 0. 

• We found that there are significant performance 
differences with allowing or not the robot to 
advance towards the node nearest to the goal 
when the trees are disconnected, as proposed in 
lfT4l . The problem is that the robot would become 
stuck if it enters a small concave zone of the 
environment(like a room in a building) while there 
are moving obstacles inside that zone, but other- 
wise it can lead to better performance. Therefore 
we present results for both kinds of behavior: 
DRRT-adv and MPRRT-adv, moves even when 
the trees are disconnected, while DRRT-noadv 
and MPRRT-noadv only moves when the trees are 
connected. 

In MP-RRT, the forest was handled simply replacing 
the oldest tree in it if the forest had reached the 
maximum size allowed. 

Concerning the parameter selection, the probability 
for selecting a point in the vicinity of a point in the 
waypoint cache in DRRT was set to 0.4 as suggested 
in |4|. The probability for trying to reuse a sub tree 
in MP-RRT was set to 0.1 as suggested in [14 J. Also, 



the forest size was set to 25 and the minimum size of 
a tree to be saved in the forest was set to 5 nodes. 

4.3. Dynamic Environment Results 

The results in tables [TJ and |2] show that it takes 
our algorithm considerably less time than it takes 
the DRRT and MP-RRT to get to the goal, with far 
less collision checks. It was expected that nearest 
neighbor lookups would be much lower in the multi- 
stage algorithm than in the other two, because they 
are only performed in the initial phase, not during 
navigation. 



Table 1 . Dynamic Environment Results, Map 1 . 



Algorithm 


Success % 


c.c. 


N.N. 


Timefs] 


Multi-stage 


99 


23502 


1122 


6.62 


DRRT-noadv 


100 


91644 


4609 


20.57 


DRRT-adv 


98 


107225 


5961 


23.72 


MP-RRT-noadv 


100 


97228 


4563 


22.18 


MP-RRT-adv 


94 


118799 


6223 


26.86 



Table 2. Dynamic Environment Results, Map 2. 



Algorithm 


Success % 


C.C. 


N.N. 


Timefs] 


Multi-stage 


100 


10318 


563 


8.05 


DRRT-noadv 


99 


134091 


4134 


69.32 


DRRT-adv 


100 


34051 


2090 


18.94 


MP-RRT-noadv 


100 


122964 


4811 


67.26 


MP-RRT-adv 


100 


25837 


2138 


16.34 



4.4. Partially Known Environment Results 

The results in tables [3] and [4] show that our multi- 
stage algorithm, although designed for dynamic envi- 
ronments, is also faster than the other two in a partially 
known environment, though not as much as in the 
previous cases. 

Table 3. Partially Known Environment Results, 
Map 1 . 



Algorithm 


Success % 


C.C. 


N.N. 


Time[s] 


Multi-stage 


100 


12204 


1225 


7.96 


DRRT-noadv 


100 


37618 


1212 


11.66 


DRRT-adv 


99 


12131 


967 


8.26 


MP-RRT-noadv 


99 


49156 


1336 


13.82 


MP-RRT-adv 


97 


26565 


1117 


11.12 



5. Conclusions 

The new multi-stage algorithm proposed here has a 
very good performance in very dynamic environments. 



Table 4. Partially Known Environment Results, 
Map 2. 



Algorithm 


Success % 


c.c. 


N.N. 


Time[s] 


Multi-stage 


100 


12388 


1613 


17.66 


DRRT-noadv 


99 


54159 


1281 


32.67 


DRRT-adv 


100 


53180 


1612 


32.54 


MP-RRT-noadv 


100 


48289 


1607 


30.64 


MP-RRT-adv 


100 


38901 


1704 


25.71 



It behaves particularly well when several small obsta- 
cles are moving around seemingly randomly. This is 
explained by the fact that if the obstacles are constantly 
moving, they will sometimes move out of the way by 
themselves, which our algorithm takes advantage of, 
but the RRT based ones do not, they just drop branches 
of the tree, that could have been useful again just a few 
moments later. 

In partially known environments the multi-stage algo- 
rithm outperforms the RRT variants, but the difference 
is not as much as in dynamic environments. 

5.1. Future Work 

There are several areas of improvement for the work 
presented in this paper. The most promising seems to 
be to experiment with different on-line planners such 
as the EP/N presented in [13], a version of the EvP(| 1 1 
and 13) modified to work in continuous configuration 
space or a potential field navigator. Also, the local 
search presented here, could benefit from the use of 
more sophisticated operators. 

Another area of research that could be tackled is 
extending this algorithm to other types of environ- 
ments, ranging from totally known and very dynamic, 
to static partially known or unknown environments. An 
extension to higher dimensional problems would be 
one logical way to go, as RRTs are know to work well 
in higher dimensions. 

Finally, as RRTs are suitable for kinodynamic plan- 
ning, we only need to adapt the on-line stage of 
the algorithm to have a new multi-stage planner for 
problem with kinodynamic constraints. 
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